#include "PSOParticle.hpp"

//#define INSERT_STOP_AT_MATCH

#define	MIN(A, B)	((A < B) ? A : B)

VND*			PSOParticle::PPVND = 0;

unsigned        PSOParticle::NRand = 1;

// static
void			PSOParticle::Init(VRPTW&	inVRPTW)
{
	if (PSOParticle::PPVND == 0)
		PSOParticle::PPVND = new VND(inVRPTW);
}

// distance euclidienne
double	PSOParticle::getDistanceBetween(const PSOParticle& in1, const PSOParticle& in2)
{
	double	distance = 0.;
	
	assert(in1.paX.size() == in2.paX.size());
	
	for (unsigned register i = 0 ; i < in1.paX.size() ; i++)
	{
		distance += pow((in1.paX[i] - in2.paX[i]), 2.);
	}
	
	return (sqrt(distance));
}


float		PSOParticle::decode()
{
	unsigned									it_cp_i = 0;
	std::multimap<double, unsigned>				customers_priorities;
	std::multimap<double, unsigned>::iterator	it_cp;
	std::multimap<double, unsigned>::iterator	it_matrix;
	std::multimap<Sol::Eval, Trio>				insert_by_costs;
	Sol::Eval									soleval;
	
	paSol.clear();
	
	
	for (unsigned i = 0 ; i < paVRPTW.size() ; i++)
		customers_priorities.insert(std::pair<double, unsigned>(paX[i], i));
		
	for (unsigned register i = 0 ; i < paVehiclesMax ; i++)
	{
		paVehicleRef[i].first = paX[paVRPTW.size() + (i << 1)];
		paVehicleRef[i].second = paX[(paVRPTW.size()) + (i << 1) + 1];
		paVehicleEndDate[i] = 0.;
	}
	
	paBadSolution = false;
	
	// for each customer
	for (it_cp = customers_priorities.begin() ; it_cp != customers_priorities.end() ; ++it_cp, it_cp_i++)
	{
		//std::cout << paSol.toString() << std::endl;
		
		// construction de la matrice
		paMatrix.clear();
		for (unsigned i = 0 ; i < paVehiclesMax ; i++)
		{
			// relative_distance a revoir
			paMatrix.insert(
				std::pair<double, unsigned>(
							relative_distance(	paVehicleRef[i].first, 
												paVehicleRef[i].second,
												paVRPTW[it_cp->second].veX,
												paVRPTW[it_cp->second].veY),
											i)
							);
		}
			
		// ajout a la solution en fonction de la matrice
		for (it_matrix = paMatrix.begin() ; it_matrix != paMatrix.end() ; ++it_matrix)
		{
			// trouver la meilleur insertion possible dans la tournee prioritaire
			Vehicle&	v = paSol.getrVehicle(it_matrix->second);
			
			insert_by_costs.clear();
			
			Vehicle::Iterator	it1;
			Vehicle::Iterator	it2;
			
			if (v.veSuccs[v.bdepot().info().siClient] == v.edepot().info().siClient)
			{
				v.insert(	it_cp->second,
							paVRPTW.size(),
							paVRPTW.size() + 1);
				paVehicleRef[it_matrix->second].first = paVRPTW[it_cp->second].veX;
				paVehicleRef[it_matrix->second].second = paVRPTW[it_cp->second].veY;
				break;
			}
			else
			{
				it1 = v.bdepot();
				it2 = it1;
				++it2;
				
				while (it2 != v.end())
				{					
					if 	(	paVRPTW.checkInsert4(	v,
													it1.info().siClient,
													it2.info().siClient,
													it_cp->second,
													v.edepot().info().siCCapacityAtArrival) != VRPTW::REALIZABLE_NONE)
					{
						v.insert(it_cp->second, it1.info().siClient, it2.info().siClient);
						paSol.eval(soleval);
						insert_by_costs.insert(	
								std::pair<Sol::Eval, Trio>( soleval,
															Trio(it_cp->second, it1.info().siClient, it2.info().siClient)));
						v.erase(it_cp->second);
#ifdef INSERT_STOP_AT_MATCH
                        break;
#endif
					}
					++it1;
					++it2;
				}
				
				if (insert_by_costs.size() == 0)
					continue;
				else
				{
					unsigned	incr = rand() % MIN(PSOParticle::NRand, insert_by_costs.size());
					std::multimap<Sol::Eval, Trio>::iterator	itm;
					
					itm = insert_by_costs.begin();
					
					for (unsigned k = 0 ; k < incr ; k++, ++itm)
						;;
					
					v.insert(itm->second.first, itm->second.second, itm->second.third);
					paVehicleRef[it_matrix->second].first = paVRPTW[it_cp->second].veX;
					paVehicleRef[it_matrix->second].second = paVRPTW[it_cp->second].veY;
					break;

				}
			}
		}
		
		
	}
	
	PPVND->execute(&paSol);
	paSol.eval(paSolEval);
    //std::cout << "end vnd : ";
    //std::cout << paSolEval.toString() << std::endl;
	return (this->getFitness());
}

double      PSOParticle::getFitness() const
{	
    if (!paSolEval.seComplete)
        return exp(100);
    
    return ((exp(paSolEval.seVehicles) * (paSolEval.seDistance)));
}
